#pragma once
#include <Eigen/Dense>
#include <vector>
#include <string>
#include <atomic>
#include <mutex>
#include <deque>

namespace industrial_robot_control {
namespace kinematics {

struct JointState {
    std::vector<double> position;  // 弧度
    std::vector<double> velocity;  // 弧度/秒
    std::vector<double> torque;    // 牛·米
};

struct CartesianPose {
    Eigen::Vector3d position;      // 米
    Eigen::Quaterniond orientation;// 四元数
};

struct TrajectoryPoint {
    JointState state;
    double time;  // 相对起始时间，秒
};

class RobotArm {
public:
    RobotArm();
    ~RobotArm() = default;
    
    // 初始化机器人参数
    bool initialize(const std::string& config_path);
    
    // 更新机器人状态
    void update();
    
    // 发送控制命令到执行器
    void sendCommands();
    
    // 运动学正解: 关节空间 -> 笛卡尔空间
    CartesianPose forwardKinematics(const std::vector<double>& joint_angles);
    
    // 运动学逆解: 笛卡尔空间 -> 关节空间
    bool inverseKinematics(const CartesianPose& target, std::vector<double>& joint_angles);
    
    // 轨迹规划: 生成平滑路径
    bool planJointTrajectory(
        const JointState& start, 
        const JointState& target, 
        double duration, 
        double sample_time = 0.01);
    
    bool planCartesianTrajectory(
        const CartesianPose& start, 
        const CartesianPose& target, 
        double duration, 
        double sample_time = 0.01);
    
    // 执行轨迹
    void executeTrajectory();
    
    // 停止运动
    void stopMotion();
    
    // 紧急停止
    void emergencyStop();
    
    // 获取当前状态
    const JointState& getCurrentState() const { return current_state_; }
    const CartesianPose& getCurrentPose() const { return current_pose_; }
    
    // 设置/获取关节限制
    void setJointLimits(const std::vector<double>& min, const std::vector<double>& max);
    const std::vector<double>& getJointMin() const { return joint_min_; }
    const std::vector<double>& getJointMax() const { return joint_max_; }
    
private:
    // 机器人参数
    int dof_ = 6;  // 6轴机器人
    Eigen::Matrix4d transform_matrix_[6];  // 连杆变换矩阵
    std::vector<double> dh_params_;  // DH参数
    
    // 关节状态
    JointState current_state_;
    CartesianPose current_pose_;
    JointState target_state_;
    
    // 轨迹数据
    std::deque<TrajectoryPoint> trajectory_;
    std::atomic<bool> executing_trajectory_;
    std::mutex trajectory_mutex_;
    
    // 关节限制
    std::vector<double> joint_min_;        // 关节最小角度
    std::vector<double> joint_max_;        // 关节最大角度
    std::vector<double> max_velocity_;     // 最大速度限制
    std::vector<double> max_acceleration_; // 最大加速度限制
    
    // 从配置文件加载参数
    bool loadParameters(const std::string& config_path);
    
    // 关节空间到笛卡尔空间的变换
    Eigen::Matrix4d computeForwardKinematics(const std::vector<double>& joint_angles);
    
    // 轨迹插值
    TrajectoryPoint interpolateTrajectory(const TrajectoryPoint& start, 
                                         const TrajectoryPoint& end, 
                                         double t);
};

} // namespace kinematics
} // namespace industrial_robot_control
